import pybullet as p
import pybullet_data

'''初始化'''
p.connect(p.GUI)  # GUI连接物理引擎
p.setGravity(0, 0, -9.8)  # 设置重力加速度
p.setAdditionalSearchPath(pybullet_data.getDataPath())  # 添加资源路径
plane_id = p.loadURDF("plane.urdf", useMaximalCoordinates=False)  # 添加地面模型
car_id = p.loadURDF("/opt/sighter/src/simulation/car.urdf",
                    basePosition=[0, 0, 0])  # 加载车辆模型

'''设置关节电机为速度控制'''
joint_ids = [0, 1, 2, 3]
for i in joint_ids:
    p.setJointMotorControl2(bodyUniqueId=car_id,
                            jointIndex=i,
                            controlMode=p.VELOCITY_CONTROL,
                            targetVelocity=0.5,
                            force=1000)

while True:
    p.stepSimulation()
